/**************************************************************************/
/*                                                                        */
/*       Copyright (c) Microsoft Corporation. All rights reserved.        */
/*                                                                        */
/*       This software is licensed under the Microsoft Software License   */
/*       Terms for Microsoft Azure RTOS. Full text of the license can be  */
/*       found in the LICENSE file at https://aka.ms/AzureRTOS_EULA       */
/*       and in the root directory of this software.                      */
/*                                                                        */
/**************************************************************************/


/**************************************************************************/
/**************************************************************************/
/**                                                                       */
/** ThreadX Component                                                     */
/**                                                                       */
/**   Thread                                                              */
/**                                                                       */
/**************************************************************************/
/**************************************************************************/


/* #define TX_SOURCE_CODE  */


/* Include necessary system files.  */

/* #include "tx_api.h" */
/* #include "tx_thread.h" */
/* #include "tx_timer.h" */


    .global     _tx_thread_execute_ptr
    .global     _tx_thread_current_ptr
    .global     _tx_timer_time_slice

#ifdef TX_ENABLE_EXECUTION_CHANGE_NOTIFY
    .global     _tx_execution_thread_enter
#endif

#define THUMB_MASK          0x20            // Thumb bit (5) of CPSR/SPSR.
#define IRQ_MODE            0x12            // IRQ mode
#define USR_MODE            0x10            // USR mode
#define SVC_MODE            0x13            // SVC mode
#define SYS_MODE            0x1F            // SYS mode
#define MODE_MASK           0x1F            // Mode mask
    .arm
    .text
    .eabi_attribute Tag_ABI_align_preserved, 1

/**************************************************************************/
/*                                                                        */
/*  FUNCTION                                               RELEASE        */
/*                                                                        */
/*    _tx_thread_schedule                             Cortex-R4/MPU/ARM   */
/*                                                           6.1          */
/*  AUTHOR                                                                */
/*                                                                        */
/*    Scott Larson, Microsoft Corporation                                 */
/*                                                                        */
/*  DESCRIPTION                                                           */
/*                                                                        */
/*    This function waits for a thread control block pointer to appear in */
/*    the _tx_thread_execute_ptr variable.  Once a thread pointer appears */
/*    in the variable, the corresponding thread is resumed.               */
/*                                                                        */
/*  INPUT                                                                 */
/*                                                                        */
/*    None                                                                */
/*                                                                        */
/*  OUTPUT                                                                */
/*                                                                        */
/*    None                                                                */
/*                                                                        */
/*  CALLS                                                                 */
/*                                                                        */
/*    None                                                                */
/*                                                                        */
/*  CALLED BY                                                             */
/*                                                                        */
/*    _tx_initialize_kernel_enter          ThreadX entry function         */
/*    _tx_thread_system_return             Return to system from thread   */
/*    _tx_thread_context_restore           Restore thread's context       */
/*                                                                        */
/*  RELEASE HISTORY                                                       */
/*                                                                        */
/*    DATE              NAME                      DESCRIPTION             */
/*                                                                        */
/*  09-30-2020     Scott Larson             Initial Version 6.1           */
/*                                                                        */
/**************************************************************************/
/* VOID   _tx_thread_schedule(VOID) */
/* { */
    .global _tx_thread_schedule
    .type   _tx_thread_schedule, "function"
_tx_thread_schedule:

    /* Enter the scheduler.  */
    SVC     0

    /* We should never get here - ever!  */
_tx_scheduler_fault__:
    B       _tx_scheduler_fault__
/* } */
/**************************************************************************/


/**************************************************************************/
/* SWI_Handler                                                            */
/**************************************************************************/

    .global SWI_Handler
    .type   SWI_Handler, "function"
    .global __tx_svc_interrupt
    .type   __tx_svc_interrupt, "function"
SWI_Handler:
__tx_svc_interrupt:
    STMFD   sp!, {r0-r3, r12, lr}           // Store the registers
    MOV     r1, sp                          // Set pointer to parameters
    MRS     r0, spsr                        // Get spsr
    STMFD   sp!, {r0, r3}                   // Store spsr onto stack and another
                                            // register to maintain 8-byte-aligned stack
    TST     r0, #THUMB_MASK                 // Occurred in Thumb state?
    LDRHNE  r0, [lr,#-2]                    // Yes: Load halfword and...
    BICNE   r0, r0, #0xFF00                 // ...extract comment field
    LDREQ   r0, [lr,#-4]                    // No: Load word and...
    BICEQ   r0, r0, #0xFF000000             // ...extract comment field

    // r0 now contains SVC number
    // r1 now contains pointer to stacked registers

    CMP     r0, #0                          // Is it a schedule request?
    BEQ     _tx_handler_svc_schedule        // Yes, go there

    CMP     r0, #1                          // Is it a system mode enter request?
    BEQ     _tx_handler_svc_super_enter     // Yes, go there

    CMP     r0, #2                          // Is it a system mode exit request?
    BEQ     _tx_handler_svc_super_exit      // Yes, go there

    LDR     r2, =0x123456
    CMP     r0, r2                          // Is it an ARM request?
    BEQ     _tx_handler_svc_arm             // Yes, go there

/**************************************************************************/
/* Unknown SVC argument                                                   */
/**************************************************************************/
    /* Unrecognized service call */
_tx_handler_svc_unrecognized:

_tx_handler_svc_unrecognized_loop:           /* We should never get here */
    B       _tx_handler_svc_unrecognized_loop


/**************************************************************************/
/* SVC 1                                                                  */
/**************************************************************************/
    /* At this point we have an SVC 1, which means we are entering
       supervisor mode to service a kernel call.  */
_tx_handler_svc_super_enter:
    // Make sure that we have been called from the system mode enter location (security).
    LDR     r2, =_txm_system_mode_enter     // Load the address of the known call point
    SUB     r1, lr, #4                      // Calculate the address of the actual call
    CMP     r1, r2                          // Did we come from txm_module_manager_user_mode_entry?
    BNE     _tx_handler_svc_unrecognized    // Return to where we came

    // Clear the user mode flag in the thread structure.
    LDR     r1, =_tx_thread_current_ptr     // Load the current thread pointer address
    LDR     r2, [r1]                        // Load current thread location from the pointer (pointer indirection)
    MOV     r1, #0                          // Load the new user mode flag value (user mode flag clear -> not user mode -> system)
    STR     r1, [r2, #0x9C]                 // Clear the current user mode selection for thread

    // Now we enter the system mode and return.
    LDMFD   sp!, {r0, r3}                   // Get spsr from the stack
    BIC     r0, r0, #MODE_MASK              // clear mode field
    ORR     r0, r0, #SYS_MODE               // system mode code
    MSR     SPSR_cxsf, r0                   // Restore the spsr

    LDR     r1, [r2, #0xA8]                 // Load the module kernel stack pointer
    CPS     #SYS_MODE                       // Switch to SYS mode
    MOV     r3, sp                          // Grab thread stack pointer
    MOV     sp, r1                          // Set SP to kernel stack pointer
    CPS     #SVC_MODE                       // Switch back to SVC mode
    STR     r3, [r2, #0xB0]                 // Save thread stack pointer
#ifndef TXM_MODULE_KERNEL_STACK_MAINTENANCE_DISABLE
    LDR     r3, [r2, #0xAC]                 // Load the module kernel stack size
    STR     r3, [r2, #20]                   // Set stack size
    LDRD    r0, r1, [r2, #0xA4]             // Load the module kernel stack start and end
    STRD    r0, r1, [r2, #0x0C]             // Set stack start and end
#endif
    LDMFD   sp!, {r0-r3, r12, pc}^          // Restore the registers and return

/**************************************************************************/
/* SVC 2                                                                  */
/**************************************************************************/
    /* At this point we have an SVC 2, which means we are exiting
       supervisor mode after servicing a kernel call.  */
_tx_handler_svc_super_exit:
    // Make sure that we have been called from the system mode exit location (security).
    LDR     r2, =_txm_system_mode_exit      // Load the address of the known call point
    SUB     r1, lr, #4                      // Calculate the address of the actual call
    CMP     r1, r2                          // Did we come from txm_module_manager_user_mode_entry?
    BNE     _tx_handler_svc_unrecognized    // Return to where we came

    // Set the user mode flag into the thread structure.
    LDR     r1, =_tx_thread_current_ptr     // Load the current thread pointer address
    LDR     r2, [r1]                        // Load the current thread location from the pointer (pointer indirection)
    MOV     r1, #1                          // Load the new user mode flag value (user mode enabled -> not system anymore)
    STR     r1, [r2, #0x9C]                 // Clear the current user mode selection for thread

    // Now we enter user mode (exit the system mode) and return.
    LDMFD   sp!, {r0, r3}                   // Get spsr from the stack
    BIC     r0, r0, #MODE_MASK              // clear mode field
    ORR     r0, r0, #USR_MODE               // user mode code
    MSR     SPSR_cxsf, r0                   // Restore the spsr

    LDR     r1, [r2, #0xB0]                 // Load the module thread stack pointer
    CPS     #SYS_MODE                       // Switch to SYS mode
    MOV     sp, r1                          // Set SP back to thread stack pointer
    CPS     #SVC_MODE                       // Switch back to SVC mode
#ifndef TXM_MODULE_KERNEL_STACK_MAINTENANCE_DISABLE
    LDR     r3, [r2, #0xBC]                 // Load the module thread stack size
    STR     r3, [r2, #20]                   // Set stack size
    LDRD    r0, r1, [r2, #0xB4]             // Load the module thread stack start and end
    STRD    r0, r1, [r2, #0x0C]             // Set stack start and end
#endif
    LDMFD   sp!, {r0-r3, r12, pc}^          // Restore the registers and return

/**************************************************************************/
/* ARM Semihosting                                                        */
/**************************************************************************/
_tx_handler_svc_arm:

    /**** TODO: handle semihosting requests or ARM angel requests ****/

    /* Just return.  */
    LDMFD   sp!, {r0, r3}                   // Get spsr from the stack
    MSR     SPSR_cxsf, r0                   // Restore the spsr
    LDMFD   sp!, {r0-r3, r12, pc}^          // Restore the registers and return

/**************************************************************************/
/* SVC 0                                                                  */
/**************************************************************************/
    /* At this point we have an SVC 0: enter the scheduler.  */
_tx_handler_svc_schedule:

    LDMFD   sp!, {r0, r3}                   // Get spsr from stack
    MSR     SPSR_cxsf, r0                   // Restore spsr
    LDMFD   sp!, {r0-r3, r12, lr}           // Restore the registers

    /* This code waits for a thread control block pointer to appear in
       the _tx_thread_execute_ptr variable.  Once a thread pointer appears
       in the variable, the corresponding thread is resumed. */

    /* Enable interrupts.  */
#ifdef TX_ENABLE_FIQ_SUPPORT
    CPSIE   if                              // Enable IRQ and FIQ interrupts
#else
    CPSIE   i                               // Enable IRQ interrupts
#endif

    /* Wait for a thread to execute.  */
    /* do */
    /* { */
    LDR     r1, =_tx_thread_execute_ptr     // Address of thread execute ptr

__tx_thread_schedule_loop:

    LDR     r0, [r1, #0]                    // Pickup next thread to execute
    CMP     r0, #0                          // Is it NULL?
    BEQ     __tx_thread_schedule_loop       // If so, keep looking for a thread

    /* } */
    /* while(_tx_thread_execute_ptr == TX_NULL); */

    /* Yes! We have a thread to execute.  Lockout interrupts and
       transfer control to it.  */

#ifdef TX_ENABLE_FIQ_SUPPORT
    CPSID   if                              // Disable IRQ and FIQ interrupts
#else
    CPSID   i                               // Disable IRQ interrupts
#endif

    /* Setup the current thread pointer.  */
    /* _tx_thread_current_ptr =  _tx_thread_execute_ptr; */

    LDR     r1, =_tx_thread_current_ptr     // Pickup address of current thread
    STR     r0, [r1, #0]                    // Setup current thread pointer

    /* Increment the run count for this thread.  */
    /* _tx_thread_current_ptr -> tx_thread_run_count++; */

    LDR     r2, [r0, #4]                    // Pickup run counter
    LDR     r3, [r0, #24]                   // Pickup time-slice for this thread
    ADD     r2, r2, #1                      // Increment thread run-counter
    STR     r2, [r0, #4]                    // Store the new run counter

    /* Setup time-slice, if present.  */
    /* _tx_timer_time_slice =  _tx_thread_current_ptr -> tx_thread_time_slice; */

    LDR     r2, =_tx_timer_time_slice       // Pickup address of time slice variable
    STR     r3, [r2, #0]                    // Setup time-slice

#ifdef TX_ENABLE_EXECUTION_CHANGE_NOTIFY

    /* Call the thread entry function to indicate the thread is executing.  */

    MOV     r5, r0                          // Save r0
    BL      _tx_execution_thread_enter      // Call the thread execution enter function
    MOV     r0, r5                          // Restore r0
#endif

    /* Determine if an interrupt frame or a synchronous task suspension frame is present. */
    CPS     #SYS_MODE                       // Enter SYS mode
    LDR     sp, [r0, #8]                    // Switch to thread stack pointer
    LDMIA   sp!, {r4, r5}                   // Pickup the stack type and saved CPSR
    CPS     #SVC_MODE                       // Enter SVC mode

    /***************************************************************************/
    /* Set up MPU for module.  */
    LDR     r1, [r0, #0x94]                 // Pickup module instance pointer
    CMP     r1, #0                          // Valid module pointer?
    BEQ     _tx_end_mpu_update              // No - skip memory protection setup
    LDR     r2, [r0, #0xA0]                 // Pickup tx_thread_module_user_mode
    CMP     r2, #1                          // In user mode?
    BNE     _tx_end_mpu_update              // No - skip memory protection setup
    /* Is the MPU already set up for this module? */
    /* Pickup the first data entry to check (txm_module_instance_mpu_registers[5]) */
    LDR     r2, [r1, #0xA0]                 // Pickup txm_module_instance_mpu_registers[5]
    MOV     r3, #5                          // Select region 5
    MCR     p15, 0, r3, c6, c2, 0           // Select region 5
    MRC     p15, 0, r3, c6, c1, 0           // Read DRBAR into r3
    CMP     r2, r3                          // Is module already loaded?
    BEQ     _tx_end_mpu_update              // Yes - skip memory protection setup

    /* Disable MPU before applying new regions. */
    MRC     p15, 0, r2, c1, c0, 0           // Read SCTLR
    BIC     r2, r2, #1                      // Disable MPU
    DSB
    MCR     p15, 0, r2, c1, c0, 0           // Write to SCTLR
    ISB
    /* Loop to load MPU registers */
    MOV     r3, #0                          // Loop index
    ADD     r1, r1, #0x64                   // Build address of MPU register table
_tx_mpu_loop:
    LDR     r2, [r1]                        // Pickup txm_module_mpu_region_address
    MCR     p15, 0, r3, c6, c2, 0           // Select region
    MCR     p15, 0, r2, c6, c1, 0           // Write to DRBAR
    ADD     r1, r1, #4                      // Increment to next MPU parameter
    LDR     r2, [r1]                        // Pickup txm_module_mpu_region_size
    MCR     p15, 0, r2, c6, c1, 2           // Write to DRSR
    ADD     r1, r1, #4                      // Increment to next MPU parameter
    LDR     r2, [r1]                        // Pickup txm_module_mpu_region_attributes
    MCR     p15, 0, r2, c6, c1, 4           // Write to DRACR
    ADD     r1, r1, #4                      // Increment to next MPU parameter
    ADD     r3, r3, #1                      // Increment loop index
    CMP     r3, #0xB                        // Check the limit
    BLE     _tx_mpu_loop                    // Loop if not finished

    /* Enable MPU with new regions. */
    MRC     p15, 0, r2, c1, c0, 0           // Read SCTLR
    ORR     r2, r2, #1                      // Enable MPU
    ORR     r2, r2, #0x20000                // Enable Background Region
    DSB
    MCR     p15, 0, r2, c1, c0, 0           // Write to SCTLR
    ISB

_tx_end_mpu_update:
    /***************************************************************************/

    CMP     r4, #0                          // Check for synchronous context switch
    BEQ     _tx_solicited_return

    MSR     SPSR_cxsf, r5                   // Setup SPSR for return
    LDR     r1, [r0, #8]                    // Get thread SP
    LDR     lr, [r1, #0x40]                 // Get thread PC
    CPS     #SYS_MODE                       // Enter SYS mode

#ifdef __ARM_FP
    LDR     r2, [r0, #144]                  // Pickup the VFP enabled flag
    CMP     r2, #0                          // Is the VFP enabled?
    BEQ     _tx_skip_interrupt_vfp_restore  // No, skip VFP interrupt restore
    VLDMIA  sp!, {D0-D15}                   // Recover D0-D15
    LDR     r4, [sp], #4                    // Pickup FPSCR
    VMSR    FPSCR, r4                       // Restore FPSCR
    CPS     #SVC_MODE                       // Enter SVC mode
    LDR     lr, [r1, #0x144]                // Get thread PC
    CPS     #SYS_MODE                       // Enter SYS mode
_tx_skip_interrupt_vfp_restore:
#endif

    LDMIA   sp!, {r0-r12, lr}               // Restore registers
    ADD     sp, sp, #4                      // Fix stack pointer
    CPS     #SVC_MODE                       // Enter SVC mode
    SUBS    pc, lr, #0                      // Return to point of thread interrupt

_tx_solicited_return:
    MOV     r2, r5                          // Move CPSR to scratch register
    CPS     #SYS_MODE                       // Enter SYS mode

#ifdef __ARM_FP
    LDR     r1, [r0, #144]                  // Pickup the VFP enabled flag
    CMP     r1, #0                          // Is the VFP enabled?
    BEQ     _tx_skip_solicited_vfp_restore  // No, skip VFP solicited restore
    VLDMIA  sp!, {D8-D15}                   // Recover D8-D15
    LDR     r4, [sp], #4                    // Pickup FPSCR
    VMSR    FPSCR, r4                       // Restore FPSCR
_tx_skip_solicited_vfp_restore:
#endif

    LDMIA   sp!, {r4-r11, lr}               // Restore registers
    MOV     r1, lr                          // Copy lr to r1 to preserve across mode change
    CPS     #SVC_MODE                       // Enter SVC mode
    TST 	r1, #1							// Test LSB to see if we're returning to Thumb mode
	ORRNE 	r2, r2, #THUMB_MASK				// If so, set Thumb bit
	BICEQ 	r2, r2, #THUMB_MASK				// If not, clear Thumb bit
    MSR     SPSR_cxsf, r2                   // Recover CPSR
    MOV  	lr, r1							// Move r1 back to lr for a proper exception return
    SUBS    pc, lr, #0                      // Return to thread synchronously

/**************************************************************************/
/* End __tx_svc_interrupt                                                 */
/**************************************************************************/





#ifdef __ARM_FP
    .global tx_thread_vfp_enable
    .type   tx_thread_vfp_enable, "function"
tx_thread_vfp_enable:
    MRS     r2, CPSR                        // Pickup the CPSR
#ifdef TX_ENABLE_FIQ_SUPPORT
    CPSID   if                              // Disable IRQ and FIQ interrupts
#else
    CPSID   i                               // Disable IRQ interrupts
#endif
    LDR     r0, =_tx_thread_current_ptr     // Build current thread pointer address
    LDR     r1, [r0]                        // Pickup current thread pointer
    CMP     r1, #0                          // Check for NULL thread pointer
    BEQ     __tx_no_thread_to_enable        // If NULL, skip VFP enable
    MOV     r0, #1                          // Build enable value
    STR     r0, [r1, #144]                  // Set the VFP enable flag (tx_thread_vfp_enable field in TX_THREAD)
__tx_no_thread_to_enable:
    MSR     CPSR_cxsf, r2                   // Recover CPSR
    BX      LR                              // Return to caller

    .global tx_thread_vfp_disable
    .type   tx_thread_vfp_disable, "function"
tx_thread_vfp_disable:
    MRS     r2, CPSR                        // Pickup the CPSR
#ifdef TX_ENABLE_FIQ_SUPPORT
    CPSID   if                              // Disable IRQ and FIQ interrupts
#else
    CPSID   i                               // Disable IRQ interrupts
#endif
    LDR     r0, =_tx_thread_current_ptr     // Build current thread pointer address
    LDR     r1, [r0]                        // Pickup current thread pointer
    CMP     r1, #0                          // Check for NULL thread pointer
    BEQ     __tx_no_thread_to_disable       // If NULL, skip VFP disable
    MOV     r0, #0                          // Build disable value
    STR     r0, [r1, #144]                  // Clear the VFP enable flag (tx_thread_vfp_enable field in TX_THREAD)
__tx_no_thread_to_disable:
    MSR     CPSR_cxsf, r2                   // Recover CPSR
    BX      LR                              // Return to caller
#endif
